#cinematic module
from numpy import *
import vtk

def DHtoHMT( DH, A ):
	"""
	DH matrix: alpha, a, theta, d
	A[0,:]=[cos(DH[0,2]), -cos(DH[0,0])*sin(DH[0,2]), sin(DH[0,0])*sin(DH[0,2]) ,DH[0,1]*cos(DH[0,2])]
	A[1,:]=[sin(DH[0,2]), cos(DH[0,0])*cos(DH[0,2]), -sin(DH[0,0])*cos(DH[0,2]),DH[0,1]*sin(DH[0,2])]
	A[2,:]=[ 0, sin(DH[0,0]), cos(DH[0,0]), DH[0,3] ]
	A[3,:]=[ 0 ,0 ,0 ,1 ]
	"""
	A.SetElement(0,0, cos(DH[0,2]))
	A.SetElement(0,1, -cos(DH[0,0])*sin(DH[0,2]))
	A.SetElement(0,2, sin(DH[0,0])*sin(DH[0,2]))
	A.SetElement(0,3, DH[0,1]*cos(DH[0,2]))
	A.SetElement(1,0, sin(DH[0,2]))
	A.SetElement(1,1, cos(DH[0,0])*cos(DH[0,2]))
	A.SetElement(1,2, -sin(DH[0,0])*cos(DH[0,2]))
	A.SetElement(1,3, DH[0,1]*sin(DH[0,2]))
	A.SetElement(2,0,0)
	A.SetElement(2,1, sin(DH[0,0]))
	A.SetElement(2,2, cos(DH[0,0]))
	A.SetElement(2,3, DH[0,3] )
	A.SetElement(3,0,0)
	A.SetElement(3,1,0)
	A.SetElement(3,2,0)
	A.SetElement(3,3,1)


	
	
